#!/usr/bin/env python
import roslib; roslib.load_manifest('Hand_Wave')
import sys
import rospy
from cv import *
import random
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from math import *
from copy import *
from Hand_Wave.msg import *
from geometry_msgs.msg import *


random.seed()

# This is the main class that runs in the ROS node. It contains the topic subscriber and runs the main callback function
# that tracks waves. It also contains an Object tracker variable and temporary image variables for frame to frame flow calculation
 
class image_converter:

    #basic init function. Assign all the instance variables
    def __init__(self):
      self.image_pub = rospy.Publisher("image_topic_2",Image)
      self.objectTracker = GboxTracker()
      self.image0 = 0
      self.image1 = 0

      NamedWindow("Image window", 1)
      self.bridge = CvBridge()
      self.image_sub = rospy.Subscriber("/wide_stereo/left/image_rect_color",Image,self.callback)

    #This is the main callback function. It converts the ROS image message data to an openCV compatable format,
    # Runs the optical Flow calculation, groups the vectors, and updates the object tracker
    def callback(self,data):
    
      #convert the ROS image data to the appropriate openCV data type using cv Bridge      
      try:
        image = self.bridge.imgmsg_to_cv(data, "mono8")
        colorImage = self.bridge.imgmsg_to_cv(data, "bgr8")
      except CvBridgeError, e:
        print e

      #if we have buffered enought images (2) then we can run the optical flow calc
      if self.image0 != 0:
        self.image1 = image
        #run optical flow
        vectors = opticFlowCalc(self.image0,self.image1, "Image window")
        #run the flow vector filter...
        offset = flowVectorFilter(vectors,1)
        #group the objects
        objects = vectorGrouping(vectors,"Image window", self.image0)
        #update the object tracker which will fire off messages for detected waves
        self.objectTracker.updateFrame(objects,offset,self.image0,"Image window",colorImage)
        self.image0 = self.image1
      else:
        self.image0 = image
      
      try:
        cv_image = self.bridge.imgmsg_to_cv(data, "mono8")
      except CvBridgeError, e:
        print e

      (cols,rows) = GetSize(cv_image)

      WaitKey(3)

      try:
        self.image_pub.publish(self.bridge.cv_to_imgmsg(cv_image, "mono8"))
      except CvBridgeError, e:
        print e


# this Function should take in a set of flow vectors, filter them, and infer some movement.
# it should return a new list of filtered flow vectors and also a trending vector

#--------------------------------THIS FUNCTION IS NOT WRITTEN... IN CASE YOU DIDN'T NOTICE----------------------
def opticFlowFilter(flowVectors):
    return 0


#this Function should take in a color image of type bgr8
#it converts this image into hsv colorspace and applies a filter
#specified by hsv_max and hsv_min and returns a binary image
#with 0xFF.. for in range values and 0 for out of range values
def skin_color(src):

    sz = GetSize(src)


    hsv_image = CreateImage( sz, 8, 3)

    hsv_mask = CreateImage( sz, IPL_DEPTH_8U, 1)

    hsv_min = Scalar(0, 30, 50, 0)
    hsv_max = Scalar(20, 190, 100, 0)


    NamedWindow( "src",1)
    ShowImage( "src", src)

    CvtColor(src, hsv_image, CV_BGR2HSV)

    NamedWindow( "hsv-img",1)
    ShowImage( "hsv-img", hsv_image)

    InRangeS (hsv_image, hsv_min, hsv_max, hsv_mask)

    NamedWindow( "hsv-msk",1)

    ShowImage( "hsv-msk", hsv_mask)
    
    return hsv_mask


#Given two images, this function attempts to find the optical flow vectors from image 1 to image 2.
#After finding all the flow vectors, it filters them by length and accumulates them into a set. 
#It then draws a pointed arrow representing the flow vector on image1 and displays it in the given window.
#This function returns a set of the filtered flow vectors.
def opticFlowCalc(image1, image2, win1):
	
      #----The section does the optical Flow Calc-----
      
      #These are constants used for optical flow calc
      max_corners = 500	
      image1Size = GetSize(image1)
      win_size = 15
      #This is the quality limit for features found in good features to track.
      #This number can be adjusted to increase or reduce the number of features
      #used in the optical flow tracking.
      feature_quality = 0.05

      #create some temporary images
      eig_image = CreateImage(image1Size, IPL_DEPTH_32F, 1)
      tmp_image = CreateImage(image1Size, IPL_DEPTH_32F, 1)
		
      #find features in image1       
      cornersA = GoodFeaturesToTrack(image1, eig_image, tmp_image, max_corners, feature_quality, 5.0)
      cornersA = FindCornerSubPix(image1, cornersA, (win_size, win_size), (-1,-1), (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03))
	
      #create some more temporary storage variables
      features_found = []
      feature_errors = []
	
      pyr_sz = (image1.width + 8, image2.height/3)	
      pyrA = CreateImage(pyr_sz, IPL_DEPTH_32F, 1)
      pyrB = CreateImage(pyr_sz, IPL_DEPTH_32F, 1)
	
      #!!Magic!!
      cornersB = []	
      (cornersB, features_found, feature_errors) = CalcOpticalFlowPyrLK (image1, image2, pyrA, pyrB, cornersA, (win_size, win_size), 5, (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03), 0)

	

      #---This section goes through all the vectors found and filters them by length and draws them on image1---
      
      # these are constants to be used in this section
      dTheta = pi/6	
      flowVectors = set()
      limit = 10
      flow_magnitude_floor = 5

      # go through all the features
      for i in range(len(features_found)):
          #calculate the vector length
          point0 = (int(cornersA[i][0]), int(cornersA[i][1]))
          point1 = (int(cornersB[i][0]), int(cornersB[i][1]))
          vector_mag = ((point0[0]-point1[0])**2 + (point0[1]-point1[1])**2)**.5

          #filter by vector length
          if ( vector_mag >= flow_magnitude_floor):
            #add the vector to the set of valid flow vectors 
            flowVectors.add((point0,point1))

            #calculate lines to be used for the arrowpoint 
            theta = atan2(point1[1]-point0[1],point1[0]-point0[0])
            thetaUp = (theta + dTheta)
            thetaDown = (theta -dTheta)
            lineLength = ((point1[1]-point0[1])**2+(point1[0]-point0[0])**2)**.5
            arrowlength = int(lineLength/3 )

            if arrowlength>limit:
               arrowlength = limit
            if arrowlength < 2:
               arrowlength = 2

            #calculate our arrowtail vectors and transformthem to align with point1
            arrowpoint1 = (int(-arrowlength*cos(thetaUp)+point1[0]),int(-arrowlength*sin(thetaUp)+point1[1]))
            arrowpoint2 = (int(-arrowlength*cos(thetaDown)+point1[0]),int(-arrowlength*sin(thetaDown)+point1[1]))

            #draw the pointed arrow onto image1 and display it in the given window 
            Line(image1, point0, point1, CV_RGB(255, 0, 0), 1, CV_AA, 0)
            Line(image1, point1, arrowpoint1, CV_RGB(255, 0, 0), 1, CV_AA, 0)
            Line(image1, point1, arrowpoint2, CV_RGB(255, 0, 0), 1, CV_AA, 0)
            

      ShowImage(win1, image1)

      return list(flowVectors)


#This function takes in a list of flow vectors which are formatted as 2-tuples of 2-tuples where the first 2-tuple is the starting point
#and the second 2-tuple is the ending point.  The function goes through the list of flow vectors
#and attempts to group the flow vetors into "blobs" or "objects" based on the starting and ending points of each vector
def vectorGrouping(flowVectors,win1,image):
    # these value will be the pixel radii of the cirlces which will threshold two vectors being in the same object
    startGroupingSize = 22
    endGroupingSize = 33

    # initialize the list of all object sets
    allObjects = []
    # initialize a set of vectors that have already been assigned
    assignedVectors = set()

    for vector1 in flowVectors:
        for vector2 in flowVectors:
	        if vector1 != vector2:

		        # first, make the object set for each vector if it has not already been made
		        if vector1 not in assignedVectors:
			        allObjects.append(set([vector1]))
			        assignedVectors.add(vector1)
		        if vector2 not in assignedVectors:
			        allObjects.append(set([vector2]))
			        assignedVectors.add(vector2)		
	
		        # For each vector pairing, compare the starting and ending points
		        # if they are within the threshold we have a match
		        startDist = ((vector1[0][0]-vector2[0][0])**2 + (vector1[0][1] - vector2[0][1])**2)**.5
		        endDist = ((vector1[1][0]-vector2[1][0])**2 + (vector1[1][1] - vector2[1][1])**2)**.5

		        if startDist<=startGroupingSize	and endDist<=endGroupingSize:
			        match = True
		        else:
			        match = False

		        # if we have a match, union their object sets
		        # otherwise, add the vectors as new object sets if they are not already in an object set
		        if match:
			        # to union the sets we first need to find the set that contains vector1
			        # if vector 2 is also in that set, we're done
			        # otherwise, we need to find the other set that contains vector2
			        # we grab it and union the two sets
			        for anObject in allObjects:
				        if vector1 in anObject:
					        if vector2 in anObject:
						        break
					        else:
						        for anotherObject in allObjects:
							        if vector2 in anotherObject:
								        #now we have the object sets that contain vectors 1 and 2
								        # we need to add the union of the two sets and remove the
								        # original object set for each vector from allObjects
								        unionSet = anObject.union(anotherObject)
								        allObjects.append(unionSet)
								        allObjects.remove(anObject)
								        allObjects.remove(anotherObject)									
								        break 
    return allObjects
    

#Given a gbox, window, and image
#it draws the gbox on the image and displays it in the window
# a helper function used to display gboxes
def drawGbox(gbox,win1,image):

    boxColor = CV_RGB(0, 0, 0)
    Rectangle(image, gbox.topLeft, gbox.bottomRight, boxColor, CV_FILLED, CV_AA, 0)
    ShowImage(win1, image)


# This function takes in a list of single Objects and a int group number count
# it should use these values to calculate an appropriate offset for the group flow vectors
# that counteracts an optical flow due to camera movement as much as possible

#---------------------------------THIS FUNCTION DOES NOT WORK WELL. REWRITE NEEDED------------------------------
def flowVectorFilter(singleObjects,groupedObjectCount):
    
    xMeanFlow = 0
    yMeanFlow = 0
    xSquaredMean = 0
    ySquaredMean = 0

    singleObjectCount = len(singleObjects)*1.0

    if groupedObjectCount > 0:
        
        scaleFactor = singleObjectCount/groupedObjectCount

        if scaleFactor >1:
            scaleFactor = 1

        for objects in singleObjects:        
            vectors = list(objects)

            x1, y1 = vectors[0]
            x2, y2 = vectors[1]

            xFlowTemp = x2 - x1
            yFlowTemp = y2 - y1
            xMeanFlow += xFlowTemp/singleObjectCount
            yMeanFlow += yFlowTemp/singleObjectCount
            xSquaredMean += xFlowTemp**2/singleObjectCount
            ySquaredMean += yFlowTemp**2/singleObjectCount
            
        xStdDv = (xSquaredMean - xMeanFlow**2)**.5
        yStdDv = (ySquaredMean - yMeanFlow**2)**.5


        finalxScaleFactor = 0
        finalyScaleFactor = 0        
        # Taking the complement of the StdDv as a proportion of the mean will
        # ensure that we get smaller offset effect for data with high variance 
        if fabs(xMeanFlow) > 2 and fabs(yMeanFlow) > 2:
            finalxScaleFactor = 1-fabs(2*xStdDv/xMeanFlow)
            finalyScaleFactor = 1-fabs(2*yStdDv/yMeanFlow)

        finalScaleFactor = finalxScaleFactor*finalyScaleFactor

        if (singleObjectCount > 20):
            finalOffsetVector = (-xMeanFlow,-yMeanFlow)
        else:
            finalOffsetVector = (0,0)

        return finalOffsetVector

    else:
        return (0,0)

#---------------------------------------------------------------------------------GBOX---------------------------------------------------


#boxes that we use for grouping
class Gbox:
    def __init__(self,hist,tl,br):
        max_count = 10
        
        self.history = hist        
        self.topLeft = tl
        self.bottomRight = br
        self.count = max_count
        self.history_max = 60
        self.probability = 0    

    #this returns a list of the corner points of the box
    def getCorners(self):
        tempPoints = []
        tempPoints.append(self.topLeft)
        tempPoints.append(self.bottomRight)
        tempPoints.append((self.topLeft[1],self.bottomRight[0]))
        tempPoints.append((self.topLeft[0],self.bottomRight[1]))
        return tempPoints

    # this returns a boolean indicating whether this box intersects the given box 'box'
    def intersects(self,box):
        intersects = False
        for corner in box.getCorners():
            intersects = intersects or self.containsPoint(corner)
        
        return intersects

    # this function returns a boolean indicating whether this box contains the given point p
    def containsPoint(self,p):
        return (p[0]>self.topLeft[0]and p[0] < self.bottomRight[0] and p[1]> self.topLeft[1] and p[1] < self.bottomRight[1])

    # this is a helper function for our init that appends the center of the current gbox to it's history
    # if our history list size > history_max, then we should also remove the oldest (first) element of
    # the list.
    def updateHistory(self):
        self.history.append(((self.topLeft[0]+self.bottomRight[0])/2,(self.topLeft[1]+self.bottomRight[1])/2))
        if (len(self.history) > self.history_max):
            self.history.pop(0)

    # This function should check if the current Gbox history is indicative of a wave.
    # It should return a tuple containing a gbox containing the wave boundaries and a probability of accuracy

    # method 1: count how many times xchanges and ychanges. If either of these counts is within our tolerance,
    # we should return a gbox with a probability indicating how certain we are that it contains a wave
    def checkWaveBox(self):
        # these values represent the range that xchange or ychange must be within to count as a wave
        waveChangeMin = 3
        waveChangeMax = 12
        
        #these accumulate the x and y valeus seperately for drawing a bounding box later
        xvalues = []
        yvalues = []

        # these values will count how many times x and y change
        xchanges = 0
        ychanges = 0

        #these values will store values from the previous iteration
        prevxDelta = 0
        prevyDelta = 0
        prevWaypoint = 0
        for waypoint in self.history:
            xvalues.append(waypoint[0])
            yvalues.append(waypoint[1])
            if prevWaypoint == 0:
                prevWaypoint = waypoint
            xDelta = waypoint[0]-prevWaypoint[0]
            yDelta = waypoint[1]-prevWaypoint[1]
            if (prevxDelta > 0 and xDelta < 0) or (prevxDelta < 0 and xDelta > 0):
                xchanges += 1
            if (prevyDelta > 0 and yDelta < 0) or (prevyDelta < 0 and yDelta > 0):
                ychanges += 1
            
            # update our prev values for the next iteration
            prevWaypoint = waypoint
            prevxDelta = xDelta
            prevyDelta = yDelta

        if ((xchanges >= waveChangeMin and xchanges <= waveChangeMax) or (ychanges >= waveChangeMin and ychanges <= waveChangeMax)):
            #If we are here, then this is a valid wave so we should build our bounding box and 
            #calculate our probabilities
            up = min(yvalues)
            down = max(yvalues)
            left = min(xvalues)
            right = max(xvalues)
            waveBox = Gbox([],(left,up),(right,down))
            prob = self.probability
            return (waveBox,prob)
        else:
            return ([],0)

    #Given an hsv binary output image associated with this Gbox the function should check the contents of the image within the gbox boundaries
    #and return the probablilty that there is a hand within the box boundaries.
    def containsHand(self,hsvResults):

        if self.isInBounds(hsvResults.width,hsvResults.height):
            skinArray = hsvResults
            #method 1: count the percentage of pixels in the box that are skin colored within predefined hsv bounds. 
            size = fabs((self.topLeft[0]-self.bottomRight[0])*(self.topLeft[0]-self.bottomRight[0]))
            skinCount = 0.0
            for x in range(self.topLeft[0],self.bottomRight[0]):
                for y in range(self.topLeft[1],self.bottomRight[1]):
                    #if the entry is not 0 then it is a positive hit
                    if x < skinArray.width and y < skinArray.height:
                        if skinArray[y,x] != 0:
                            skinCount += 1

            #method 2: generate a contour and convex hull for the bounding box. Then, using convexity defects, find the defects and see
            #if there are about 4 or 5. 

            # first select the region of interest in the hsv mask to be the area of the bounding bx
            rect = (int(self.topLeft[0]),int(self.topLeft[1]),int(fabs(self.bottomRight[0]-self.topLeft[0])),int(fabs(self.topLeft[1]-self.bottomRight[1])))
       
            SetImageROI(skinArray,rect)
            contours = FindContours(skinArray, CreateMemStorage(),CV_RETR_TREE,CV_CHAIN_APPROX_SIMPLE)
            hull = []
            defects = []

            if len(contours) > 0:
                #DrawContours(hsv_mask,contours,RGB(0,0,0),RGB(50,50,50),0)
                hull = ConvexHull2(contours,CreateMemStorage())
                defects = ConvexityDefects(contours,hull,CreateMemStorage())

            ResetImageROI(skinArray)

            
            skinScore = skinCount/size
            if (len(defects) == 5):
                defectScore = 1
            else:        
                defectScore = 1/(fabs(5-len(defects)))
            totalScore = sum((skinScore,defectScore))/2
            
            self.probability = (self.probability + totalScore)/2.0

            return totalScore
        else:
            return 0
    
    #This function will return True if the gbox is within the given boudary limits
    def isInBounds(self,width,height):
        up = self.topLeft[1]
        left = self.topLeft[0]
        right = self.bottomRight[0]
        down = self.bottomRight[1]
        return up<height and down<height and right < width and left<width and up>0 and down>0 and right>0 and left>0


    #This function will update the corners and each history element
    # by adding the opposite of the offset vector 
    def calibrateForOffset(self,offsetVector):
        realOffset = (-offsetVector[0],-offsetVector[1])
        self.topLeft = (int(self.topLeft[0]+realOffset[0]),int(self.topLeft[1]+realOffset[1]))
        self.bottomRight = (int(self.bottomRight[0]+realOffset[0]),int(self.bottomRight[1]+realOffset[1]))

        for point in self.history:
            point = (int(point[0]+realOffset[0]),int(point[1]+realOffset[1]))
        
    def markForCleanup(self):
        self.count = 0

    def decrementCount(self):
        self.count -= 1

    def isValid(self):
        return self.count > 0
        


#---------------------------------------------------------------------------------GBOXTRACKER---------------------------------------------------


class GboxTracker:

    def __init__(self):
        self.gboxes = []
    
    # this function should take in a list of "Objects" (which are lists of flows vectors that have been grouped together in sets)
    # and update the current gBoxes list to reflect this new frame. Each object should either get a new gbox or should inherit
    # the history of an existing gbox
    def updateFrame(self, allObjects,offset,image,win1,colorImage):
        # first, iterate through the objects list to find the bounding corners of the object
        # and construct a gbox list that contains 1 Gbox tuple (gbox1,gbox2) per object
        # now that we have our object groupings we can draw bounding boxes around each group's starting and ending points

        #limit is the pixel padding for the bounding box surrounding each object
        limit = 5
        #this is our list of Gbox tuples that we shall use to update our frame
        newBoxPairs = []

        singleObjects = []
        for anObject in allObjects:
            if (len(anObject) < 2) and (len(anObject) > 0):
                singleObjects.append(anObject)
            else:
                tempBoxPair = []
                for i in range(0,2):
                    # first find the corner points for the object
                    up    = float('inf')
                    down  = -float('inf')
                    left  = float('inf')
                    right = -float('inf')
                    for vector in anObject:
                        x,y = vector[i]

                        if up > y:
	                        up = y
                        if down < y:
	                        down = y
                        if left > x:
	                        left = x
                        if right < x:
	                        right = x

                    up = up - limit
                    down = down + limit
                    left = left - limit
                    right = right + limit
                    
                    #make a new Gbox and add it to our temp box pair to be appened to our newBoxPairs
                    if (up<image.height and down<image.height and right < image.width and left<image.width):
                        if (up>0 and down>0 and right>0 and left>0):                    
                            tempBoxPair.append(Gbox([],(left,up),(right,down)))

                    #draw it
                    boxColor = CV_RGB(0, 255, 0)
                    if i == 1:
                        boxColor = CV_RGB(0, 0, 255)

                    Line(image, (left,up), (right,up), boxColor, 1, CV_AA, 0)
                    Line(image, (left,up), (left,down), boxColor, 1, CV_AA, 0)
                    Line(image, (left,down), (right,down), boxColor, 1, CV_AA, 0)
                    Line(image, (right,up), (right,down), boxColor, 1, CV_AA, 0)
                if (len(tempBoxPair) == 2):
                    newBoxPairs.append(tuple(tempBoxPair))

        ShowImage(win1, image)

        #offsetVector = flowVectorFilter(singleObjects,len(allObjects)-len(singleObjects))
        offsetVector = offset
        if len(singleObjects) > 1:        
            self.calibrateForOffset(offsetVector,newBoxPairs)

        #Now, filter this list for hands on gbox2 with the given image
        #we need to call skin_color first to produce the hsv output we need for filter hands
        tempResults = skin_color(colorImage)        
        hsvResults = CreateImage( GetSize(tempResults), IPL_DEPTH_8U, 1)
        Smooth(tempResults,hsvResults)
        newBoxPairsF = self.filterForHands(newBoxPairs,hsvResults)

        #Now, using our filtered list(newBoxPairsF) we should check each gbox1 to see if it is contained in
        #or a likely successor of any of our existing gboxes
        tempNewBoxes = []
        for aGboxPair in newBoxPairsF:
            gbox1 = aGboxPair[0]
            gbox2 = aGboxPair[1]
            newObject = True
            for oldGbox in self.gboxes:
                if oldGbox.intersects(gbox1):
                    #if the boxes intersect, then make a new Gbox with the history of the oldGbox
                    # and the corners of the current gbox2 and append it to the Gboxes list
                    #Also, mark the oldGbox for cleanup
                    tempGbox = Gbox(oldGbox.history,gbox2.topLeft,gbox2.bottomRight)
                    tempGbox.updateHistory()
                    tempGbox.probability = gbox2.probability
                    tempNewBoxes.append(tempGbox)
                    oldGbox.markForCleanup()
                    newObject = False
            if newObject:
                # if there were no intersections then this gbox pair is a new object and we should add it to
                # the gboxes list with an empty history
                tempGbox = Gbox([],gbox2.topLeft,gbox2.bottomRight)
                tempGbox.updateHistory()
                tempNewBoxes.append(tempGbox)

        # update the counts and clean up marked boxes
        self.updateCount()

        #take our tempNewBoxes list and append all it's boxes to the real gboxes list
        for tempbox in tempNewBoxes:
            self.gboxes.append(tempbox)

        boxColor = CV_RGB(0, 0, 0)
        for gbox in self.gboxes:
            Rectangle(image, gbox.topLeft, gbox.bottomRight, boxColor, 1, CV_AA, 0)
            ShowImage(win1, image)

        self.drawPaths(win1,image)
        waveBoxes = self.getWaves()
        self.publishMessage(waveBoxes,'hand_wave')
        for box,prob in waveBoxes:
            drawGbox(box,win1,image)
            
    #this function publishes messages to the registered topic
    def publishMessage(self, waveBoxes,eventType):
        referenceArea = 200
        referenceDist = 4

        event = interest_events()
        event.event_position = []
        event.confidence = []		
        event.event_type = []        
        for box,prob in waveBoxes:
            boxArea = fabs(box.topLeft[0]-box.bottomRight[0])*fabs(box.topLeft[1]-box.bottomRight[1])
            pointS = PointStamped()
            pointS.header.frame_id = "wide_stereo_optical_frame"
            pointS.point.x = boxArea/referenceArea*referenceDist
            pointS.point.y = box.history[0][0]
            pointS.point.z = box.history[0][1]

            event.event_position.append(pointS)
            event.confidence.append(prob)		
            event.event_type.append(eventType)

        pub.publish(event)

    # this function should decrement the counts for each gbox in the List and remove any gboxes that have a count <= 0
    def updateCount(self):
        tempboxes = []
        for gbox in self.gboxes:
            gbox.decrementCount()
            if gbox.isValid():
                tempboxes.append(gbox)        
        self.gboxes = tempboxes

    #this function will take in an offset vector and update the existing objects to reflect this offset
    #it will also take in a seperate list of new Objects that it will adjust the flow vectors for and return
    def calibrateForOffset(self,offsetVector,objects):
        for gbox in self.gboxes:
            gbox.calibrateForOffset(offsetVector)
        
        for anObject in objects:
            #tempObject = list(anObject)
            #point1 = tempObject[0][0]
            #point2 = (tempObject[0][1][0]+offsetVector[0],tempObject[0][1][1]+offsetVector[1])
            box1 = anObject[0]
            box2 = anObject[1]
            box2.calibrateForOffset((-offsetVector[0],-offsetVector[1]))
            anObject = (box1,box2)

        return objects

    # this is a utility function that is used to draw the paths for every valid gbox object in our gboxes list.
    # it should draw a small circle centered at each waypoint and a line connecting each waypoint in the gbox history
    def drawPaths(self,win1,image):
        circleSize = 10
        lineColor = CV_RGB(0,0,0)

        for gbox in self.gboxes:
            oldPoint = 0
            for point in gbox.history:
                if oldPoint == 0:
                    oldPoint = point

                Circle(image, point, circleSize, 255)
                Line(image, oldPoint, point, lineColor, 1, CV_AA, 0)
                
                oldPoint = point      
        ShowImage(win1, image)

    #This function should return a list of wave boxes and associated probabilities by walking through the gboxes and
    #checking for waves
    def getWaves(self):
        waveList = []
        for gbox in self.gboxes:
            tempWave = gbox.checkWaveBox()

            if tempWave[0] != []:
                tempWave[0].updateHistory()
            if tempWave[1] != 0:
                waveList.append(tempWave)
        return waveList

    #Given a list of objects and an associated image.
    #This function should check each box2 for hands. And if the hand probability is above the tolerance threshold
    #it should add the associatted object to the return list. when it has checked all of the objects, it should return
    # the list containing the hand-positive objects 
    def filterForHands(self,testObj,hsvResults):
        handCutoff = .4
        handObjects = []
        for gbox1,gbox2 in testObj:
            
            results = gbox2.containsHand(hsvResults)
            if results>handCutoff:
                handObjects.append((gbox1,gbox2))
        
        return handObjects

def main(args):
    ic = image_converter()
    rospy.init_node('image_converter', anonymous=True)
    try:
      rospy.spin()
    except KeyboardInterrupt:
      print "Shutting down"
    DestroyAllWindows()

if __name__ == '__main__':


    pub = rospy.Publisher('interest_events', interest_events)
    main(sys.argv)

